#include <stdio.h>
#include <unistd.h>

#include <chrono>
#include <cmath>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <sensor_msgs/msg/nav_sat_status.hpp>
#include <sensor_msgs/msg/imu.hpp>

#include "serial_interface_linux.h"
#include "witdatapkg.h"

struct Quaternion {
  double w, x, y, z;
};

struct EulerAngles {
    double roll, pitch, yaw;  // unit is radin
};

Quaternion ToQuaternion(double yaw, double pitch, double roll) {
	// yaw (Z), pitch (Y), roll (X)
	// Abbreviations for the various angular functions
	double cy = cos(yaw * 0.5);
	double sy = sin(yaw * 0.5);
	double cp = cos(pitch * 0.5);
	double sp = sin(pitch * 0.5);
	double cr = cos(roll * 0.5);
	double sr = sin(roll * 0.5);

	Quaternion q;
	q.w = cy * cp * cr + sy * sp * sr;
	q.x = cy * cp * sr - sy * sp * cr;
	q.y = sy * cp * sr + cy * sp * cr;
	q.z = sy * cp * cr - cy * sp * sr;

	return q;
}

EulerAngles ToEulerAngles(Quaternion q) {
	EulerAngles angles;

	// roll (x-axis rotation)
	double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
	double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
	angles.roll = std::atan2(sinr_cosp, cosr_cosp);

	// pitch (y-axis rotation)
	double sinp = 2 * (q.w * q.y - q.z * q.x);
	if (std::abs(sinp) >= 1)
		angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
	else
		angles.pitch = std::asin(sinp);

	// yaw (z-axis rotation)
	double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
	double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
	angles.yaw = std::atan2(siny_cosp, cosy_cosp);

	return angles;
}


class WitDeviceNode : public rclcpp::Node {
 public:
  explicit WitDeviceNode(std::string name, std::string port_name,
                        uint32_t port_baudrate)
      : Node(name), node_name_(name) {
    RCLCPP_INFO(this->get_logger(), "node start: %s", node_name_.c_str());

    serial_ = new SerialInterfaceLinux();

    wtgahrs1pkg_ = new wit_private_msg::WitDataPkg();

    serial_->SetReadCallback(
        std::bind(&wit_private_msg::WitDataPkg::RecvDataCallback, wtgahrs1pkg_,
                  std::placeholders::_1, std::placeholders::_2));

    if (!serial_->Open(port_name, port_baudrate)) {
      RCLCPP_ERROR(this->get_logger(), "open comm port error");
      // exit(EXIT_FAILURE);
    } else {
			imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("WTGAHRS1_IMU", 10);

			gnss_pub_ = this->create_publisher<sensor_msgs::msg::NavSatFix>("WTGAHRS1_GNSS", 10);

      using namespace std::literals::chrono_literals;
      timer_ = this->create_wall_timer(
          10ms, std::bind(&WitDeviceNode::TimerCallback, this));
    }
  }

  ~WitDeviceNode() {
    RCLCPP_INFO(this->get_logger(), "node end: %s", node_name_.c_str());
  }

 private:
  void TimerCallback(void) {
    if (wtgahrs1pkg_->IsImuDataReady()) {
      // RCLCPP_INFO(this->get_logger(), "IMU DATA");
      wit_private_msg::WitImuAccelerometerType acc = wtgahrs1pkg_->GetImuAccelerometerData();
      wit_private_msg::WitImuAngularvelType angularvel = wtgahrs1pkg_->GetImuAngularvelData();
      wit_private_msg::WitImuAngularType angular = wtgahrs1pkg_->GetImuAngularData();
      // RCLCPP_INFO(
      //     this->get_logger(),
      //     "acc_x: %f (g), acc_y: %f (g), acc_z: %f (g); angle_vel_x: %f "
      //     "(deg/s), angle_vel_y: %f (deg/s), angle_vel_z: %f (deg/s); roll-x: "
      //     "%f (deg), pitch-y: %f (deg), yaw-z: %f (deg)\n",
      //     acc.acc_x, acc.acc_y, acc.acc_z, angularvel.angularvel_x,
      //     angularvel.angularvel_y, angularvel.angularvel_z, angular.roll_x,
      //     angular.pitch_y, angular.yaw_z);
      // RCLCPP_INFO(this->get_logger(), "-------------------------");
			sensor_msgs::msg::Imu imu_msg;

			imu_msg.header.stamp = this->now();
			imu_msg.header.frame_id = "wtgahrs1_imu_frame";

			// set accelerations, unit is m/s^2
			imu_msg.linear_acceleration.x = (double)acc.acc_x * 9.8;
			imu_msg.linear_acceleration.y = (double)acc.acc_y * 9.8;
			imu_msg.linear_acceleration.z = (double)acc.acc_z * 9.8;

			// set angularvel, unit is radin/s
			imu_msg.angular_velocity.x = (double)angularvel.angularvel_x * 3.14159265 / 180.0;
			imu_msg.angular_velocity.y = (double)angularvel.angularvel_y * 3.14159265 / 180.0;
			imu_msg.angular_velocity.z = (double)angularvel.angularvel_z * 3.14159265 / 180.0;

			// set angular quaternion
			EulerAngles pose_angle;
			pose_angle.roll = (double)angular.angular_x * 3.14159265 / 180.0;
			pose_angle.pitch = (double)angular.angular_y * 3.14159265 / 180.0;
			pose_angle.yaw = (double)angular.angular_z * 3.14159265 / 180.0;

			// test
			// double Roll, Pitch, Yaw, Compass;
			// Roll = (double)angular.angular_x;
			// Pitch = (double)angular.angular_y;
			// Yaw = (double)angular.angular_z;
			// if (Yaw < 0) {
			// 	Yaw = 360 + Yaw;  // 右手系
			// }
			// Compass = 360 - Yaw; // 左手系

			// Compass = Compass - 90; // 正Y 向左旋转90度
			// if (Compass < 0) Compass += 360;

			// Roll = -1 * Roll;  // 右手系 逆时针 + 顺时针 -

			// RCLCPP_INFO(this->get_logger(), "RPY: roll:%f(deg), pitch:%f(deg), Yaw:%f(deg), compass:%f(deg)", Roll, Pitch, Yaw, Compass);


			Quaternion pose_quaternion = ToQuaternion(pose_angle.yaw, pose_angle.pitch, pose_angle.roll);
			imu_msg.orientation.x = pose_quaternion.x;
			imu_msg.orientation.y = pose_quaternion.y;
			imu_msg.orientation.z = pose_quaternion.z;
			imu_msg.orientation.w = pose_quaternion.w;

			imu_pub_->publish(imu_msg);
    }

    if (wtgahrs1pkg_->IsGnssDataReady()) {
      // RCLCPP_INFO(this->get_logger(), "GNSS DATA");
      wit_private_msg::WitGnssLocationType gnssdata = wtgahrs1pkg_->GetGnssLocationData();
      // RCLCPP_INFO(
      //     this->get_logger(),
      //     "lng: %f (deg), lat: %f (deg), speed: %d (0.1m/s), altitude: %d (m)",
      //     gnssdata.current_pos_lng, gnssdata.current_pos_lat,
      //     gnssdata.actual_speed, gnssdata.actual_altitude);
      // RCLCPP_INFO(this->get_logger(), "-------------------------");
			sensor_msgs::msg::NavSatFix gnss_msg;

			gnss_msg.header.stamp = this->now();
			gnss_msg.header.frame_id = "wtgahrs1_gnss_frame";
			sensor_msgs::msg::NavSatStatus sta;
			sta.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX;
			sta.service = sensor_msgs::msg::NavSatStatus::SERVICE_COMPASS;
			gnss_msg.status = sta;
			gnss_msg.latitude = (double)gnssdata.current_pos_lat; // 纬度
			gnss_msg.longitude = (double)gnssdata.current_pos_lng; // 经度
			gnss_msg.altitude = (double)gnssdata.actual_altitude;  // 海拔
			gnss_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
			gnss_msg.position_covariance.fill(0.0);

			gnss_pub_->publish(gnss_msg);
    }
  }

  SerialInterfaceLinux* serial_;
  wit_private_msg::WitDataPkg* wtgahrs1pkg_;
  rclcpp::TimerBase::SharedPtr timer_;
  std::string node_name_;
	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
	rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gnss_pub_;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);

  auto node = std::make_shared<WitDeviceNode>("WTGAHRS1", "/dev/ttyUSB0", 9600);
  rclcpp::spin(node);
  rclcpp::shutdown();

  return 0;
}
